#pragma once

#include <string>
#include <vector>
#include "Eigen/Geometry"

using namespace std;
using namespace Eigen;

//Indexes for nodes
enum SkeletalNodes 
{
	//nodeHipCenter,
	nodeSpine,
	nodeShoulderCenter,
	nodeHead,
	nodeShoulderLeft,
	nodeElbowLeft,
	nodeWristLeft,
	nodeHandLeft,
	nodeShoulderRight,
	nodeElbowRight,
	nodeWristRight,
	nodeHandRight,
	nodeHipLeft,
	nodeKneeLeft,
	nodeAnkleLeft,
	//nodeFootLeft,
	nodeHipRight,
	nodeKneeRight,
	nodeAnkleRight,
	//nodeFootRight,
	SKELETAL_NODE_COUNT // #of nodes that are scanned with Kinect
};

#define DENIED_NODE 3

//Skeletal Nodes
class SkeletalNode
{
	//Attributes
	string name;
	Vector4f position;
public:
	//Constructors and Destructor
	SkeletalNode();
	SkeletalNode(const char* Name, const Vector4f& Position);
	~SkeletalNode();

	//getters
	const string getName(void)const;
	const Vector4f* getPosition(void)const;

	//setters
	void setName(const char* Name);
	void setPosition(const Vector4f& Position);
	void setPosition(const float x, const float y, const float z, const float w);

	//Some weird macro definition about EIGEN
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

//Skeleton that defined in every frame
class Skeleton
{
	//Attributes
	vector<SkeletalNode*> nodes;
public:
	//Constructors and Destructor
	Skeleton(void);
	~Skeleton(void);

	//getter & setter
	void addSkeletalNode(const SkeletalNodes Index, const char* Name, const Vector4f& Position);
	const SkeletalNode* getSkeletalNode(const SkeletalNodes index)const;
	const SkeletalNode* getSkeletalNode(const int index)const;
	void setSkeletalNode(const SkeletalNodes index, float x, float y, float z, float w);
};


/************************************************************************/
/*                  Skeletal Node Methods                               */
/************************************************************************/

inline const string SkeletalNode::getName(void)const
{
	return name;
}

inline const Vector4f* SkeletalNode::getPosition(void)const 
{
	return &position;
}

inline void SkeletalNode::setName(const char* Name)
{
	this->name = Name;
}

inline void SkeletalNode::setPosition(const Vector4f& Position)
{
	position = Position;
}

inline void SkeletalNode::setPosition(const float x, const float y, const float z, const float w)
{
	position[0] = x;
	position[1] = y;
	position[2] = z;
	position[3] = w;
}

/************************************************************************/
/*                  Skeleton Methods                                    */
/************************************************************************/

inline void Skeleton::addSkeletalNode(const SkeletalNodes Index, const char* Name, const Vector4f& Position)
{ 
	nodes[Index] = new SkeletalNode(Name,Position);
}

inline const SkeletalNode* Skeleton::getSkeletalNode(const SkeletalNodes index) const
{
	return nodes[index];
}

inline const SkeletalNode* Skeleton::getSkeletalNode(const int index) const
{
	return nodes[index];
}

inline void Skeleton::setSkeletalNode( const SkeletalNodes index, float x, float y, float z, float w )
{
	nodes[index]->setPosition(x,y,z,w);
}
